/*---------------------------------------------------------------------*\
    This program comes with ABSOLUTELY NO WARRANTY!
    This is free software, and you are welcome to redistribute it
    under the conditions of the GPLv3.
\*---------------------------------------------------------------------*/

#include "opencv/cv.h"
#include "opencv/highgui.h"
#include <stdio.h>
#include <ctype.h>

// -------------------------------------------------------------------------
double quality(const CvScalar &t, const CvScalar &f)
{
  return (t.val[0] > f.val[0] ? (t.val[0]/f.val[0]):(f.val[0]/t.val[0])) +
         (t.val[1] > f.val[1] ? (t.val[1]/f.val[1]):(f.val[1]/t.val[1])) +
         (t.val[2] > f.val[2] ? (t.val[2]/f.val[2]):(f.val[2]/t.val[2])) -
          3.0;
}

// -------------------------------------------------------------------------
CvScalar rgbSum(IplImage *image,int x, int y, int width, int height)
{
  int nc  = image->nChannels;
  int step= image->widthStep;

  unsigned char *data= reinterpret_cast<unsigned char *>
                                       (image->imageData);


  CvScalar ret;
  ret.val[0] = ret.val[1] = ret.val[2] = 0;

  for (int i=0; i<height; i++) {
    for (int j=0; j<width; j++) {
      int firstIndex = (y+i)*step + (x+j)*nc;
      ret.val[0] += data[firstIndex + 0];
      ret.val[1] += data[firstIndex + 1];
      ret.val[2] += data[firstIndex + 2];
    }
  }
  ret.val[0] /= height*width;
  ret.val[1] /= height*width;
  ret.val[2] /= height*width;
  return ret;
}

// -------------------------------------------------------------------------
int main( int argc, char** argv )
{
  CvCapture	*capture = 0;
  IplImage	*frame 	 = 0;	// grabbed frame
  IplImage      *result  = 0;
  
  capture = cvCaptureFromCAM(0);
  if (!capture) {
    fprintf(stderr,"Error: Could not initialize capturing!\n");
    fprintf(stderr,"plz connect usb cam..\n");
    return -1;
  }
  cvSetCaptureProperty(capture, CV_CAP_PROP_FRAME_WIDTH,320);
  cvSetCaptureProperty(capture, CV_CAP_PROP_FRAME_HEIGHT,240);


  IplImage  *target = cvLoadImage(argv[1]);
  if (!target) {
    fprintf(stderr,"Error: Could not load target image\n");
    fprintf(stderr," usage: videomatch target.png\n");
    return -1;
  }

  // create "reference-sum"
  CvScalar targetSum = rgbSum(target,0,0,target->width,target->height); 

  // create debug-window
  cvNamedWindow( "Capture", CV_WINDOW_AUTOSIZE );
  cvMoveWindow( "Capture", 0, 0 );

  while (1) {
	int key;
   
	// skip some frames.. 
        cvQueryFrame( capture );

	// capture a frame
        frame = cvQueryFrame( capture );
    	if( !frame )
        break;		    
	    
        // create result-Image if needed:
        if (!result) {
          CvSize resultSize;
          resultSize.width  = frame->width  - target->width  + 1;
          resultSize.height = frame->height - target->height + 1;
          result = cvCreateImage(resultSize, IPL_DEPTH_32F, 1);
        }

        // Match:
        cvMatchTemplate(frame, target, result, 0);
        cvNormalize(result,result, 0, 1, CV_MINMAX,  NULL);
        double min,max;
        CvPoint minLoc, maxLoc;
        cvMinMaxLoc(result, &min, &max, &minLoc, &maxLoc, NULL);

        // check match-quality:
        CvScalar frameSum = rgbSum(frame,minLoc.x,minLoc.y,target->width,target->height);
        double q = quality(targetSum,frameSum);

        // if quality is good (best is "0.0"), draw target-cross:
        if (q < 0.5) {
          int x = minLoc.x + target->width/2;
          int y = minLoc.y + target->height/2;
          cvLine(frame, cvPoint(0,y), cvPoint(frame->width-1,y), cvScalar(255,0,0,0), 5);
          cvLine(frame, cvPoint(x,0), cvPoint(x,frame->height-1), cvScalar(255,0,0,0), 5);
        }

        // show image:
	cvShowImage( "Capture", frame );
  
        // give cpu some time..
        key = cvWaitKey( 50 );
        if( key == 1048603 )
            break;	
  }

  cvReleaseCapture( &capture );
  cvDestroyWindow( "Capture" );
  return 0;
}
